#include "Sem.h"
#include "Irq.h"
#include "Task.h"

void InitSemaphore(SemaphoreType* sem, int count)
{
    sem->count = count;
    InitList(&sem->waitList);
}

// 
void WaitSemaphore(SemaphoreType* sem)
{
    uint32_t eflags = DisableInterrupts();
    if (sem->count > 0)
    {
        --sem->count;
    }
    else
    {
        TaskType* current = GetCurrentTask();
        SetTaskBlock(current);
        SetTaskWait(current, sem);
        DispatchTask();
    }
    EnableInterrupts(eflags);
}

void NotifySemaphore(SemaphoreType* sem)
{
    uint32_t eflags = DisableInterrupts();
    if (GetListCount(&sem->waitList)>0)
    {
        ListNodeType* node = GetFirstNode(&sem->waitList);
        TaskType* task = NodeParent(node, TaskType, waitNode);
        SetTaskReady(task);
        DispatchTask();
    }
    else
    {
        ++sem->count;
    }
    EnableInterrupts(eflags);
}

int GetSemaphoreCount(SemaphoreType* sem)
{
    uint32_t eflags = DisableInterrupts();
    int value = sem->count;
    EnableInterrupts(eflags);
    return value;
}